/*****************************************************************************
*                                                                            *
*  OpenNI 2.x Alpha                                                          *
*  Copyright (C) 2012 PrimeSense Ltd.                                        *
*                                                                            *
*  This file is part of OpenNI.                                              *
*                                                                            *
*  Licensed under the Apache License, Version 2.0 (the "License");           *
*  you may not use this file except in compliance with the License.          *
*  You may obtain a copy of the License at                                   *
*                                                                            *
*      http://www.apache.org/licenses/LICENSE-2.0                            *
*                                                                            *
*  Unless required by applicable law or agreed to in writing, software       *
*  distributed under the License is distributed on an "AS IS" BASIS,         *
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
*  See the License for the specific language governing permissions and       *
*  limitations under the License.                                            *
*                                                                            *
*****************************************************************************/
#include "process_depth_data.h"

using namespace openni;

void getCameraParams(openni::Device& Device, CAMERA_INTRINSIC_PARAMETERS& cam_para);
void print_pixel(Mat &src);

int main(int argc, char* argv[])
{
	ProcessData processs_depth;
	processs_depth.open_device_orbbec();
	processs_depth.get_camera_params_orbbec();

	cv::Mat depth_mat;
	while(true) {
		processs_depth.start_process_orbbec(depth_mat);
		processs_depth.process_depth_data(depth_mat);
	}

}

int main_old(int argc, char* argv[])
{
	Status rc = OpenNI::initialize();
	if (rc != STATUS_OK) {
		printf("Initialize failed\n%s\n", OpenNI::getExtendedError());
		return 1;
	}
	ProcessData processs_depth;

	Device device;
    if (argc < 2)
        rc = device.open(ANY_DEVICE);
    else
        rc = device.open(argv[1]);
    if (rc != STATUS_OK) {
		printf("Couldn't open device\n%s\n", OpenNI::getExtendedError());
		return 2;
	}

	VideoStream depth;
	if (device.getSensorInfo(SENSOR_DEPTH) != NULL) {
		//cout<<device.getSensorInfo(SENSOR_DEPTH)<<endl;
		rc = depth.create(device, SENSOR_DEPTH);
		if (rc != STATUS_OK) {
			printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError());
			return 3;
		}
	}

	rc = depth.start();
	if (rc != STATUS_OK) {
		printf("Couldn't start the depth stream\n%s\n", OpenNI::getExtendedError());
		return 4;
	}

	CAMERA_INTRINSIC_PARAMETERS cam_para;
	getCameraParams(device, cam_para);

	VideoFrameRef frame;
	cv::Mat depth_view;

	while (true) {
		int changedStreamDummy;
		VideoStream* pStream = &depth;
		rc = OpenNI::waitForAnyStream(&pStream, 1, &changedStreamDummy, SAMPLE_READ_WAIT_TIMEOUT);
		if (rc != STATUS_OK) {
			printf("Wait failed! (timeout is %d ms)\n%s\n", SAMPLE_READ_WAIT_TIMEOUT, OpenNI::getExtendedError());
			continue;
		}

		rc = depth.readFrame(&frame);
		if (rc != STATUS_OK) {
			printf("Read failed!\n%s\n", OpenNI::getExtendedError());
			continue;
		}

		if (frame.getVideoMode().getPixelFormat() != PIXEL_FORMAT_DEPTH_1_MM &&
			frame.getVideoMode().getPixelFormat() != PIXEL_FORMAT_DEPTH_100_UM) {
			printf("Unexpected frame format\n");
			continue;
		}

		DepthPixel* pDepth = (DepthPixel*)frame.getData();
		int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2;
		//printf("[%04llu] mid_val: %d, mid: %d\n", (long)frame.getTimestamp(), pDepth[middleIndex], middleIndex);

		const int w = frame.getWidth();
		const int h = frame.getHeight();
		cv::Mat img_depth(cv::Size(w, h), CV_16UC1, (DepthPixel*)frame.getData(), cv::Mat::AUTO_STEP);
        printf("depth width: %d, height: %d, mid_val: %d\n", w, h, img_depth.at<short>(w/2,h/2));

        cv::convertScaleAbs(img_depth, depth_view, 0.03, 0);
        cv::applyColorMap(depth_view, depth_view, cv::COLORMAP_JET);
        cv::imshow("depth_view", depth_view);

		processs_depth.process_depth_data(img_depth);
		//print_pixel(img_depth);
        int key = cv::waitKey(1);
        if(key == 'q') break;
	}

	depth.stop();
	depth.destroy();
	device.close();
	OpenNI::shutdown();

	return 0;
}

#if 1
void getCameraParams(openni::Device& device, CAMERA_INTRINSIC_PARAMETERS& cam_para)
{
    OBCameraParams cameraParam;
    int dataSize = sizeof(cameraParam);
    memset(&cameraParam, 0, sizeof(cameraParam));
    openni::Status rc = device.getProperty(openni::OBEXTENSION_ID_CAM_PARAMS, (uint8_t *)&cameraParam, &dataSize);
    if (rc != openni::STATUS_OK) {
        std::cout << "Error:" << openni::OpenNI::getExtendedError() << std::endl;
        return;
    }
    cam_para.fx = cameraParam.l_intr_p[0]; 
    cam_para.fy = cameraParam.l_intr_p[1]; 
    cam_para.cx = cameraParam.l_intr_p[2];
    cam_para.cy = cameraParam.l_intr_p[3];
    std::cout << "cam_para.fx = " << cam_para.fx << std::endl;
    std::cout << "cam_para.fy = " << cam_para.fy << std::endl;
    std::cout << "cam_para.cx = " << cam_para.cx << std::endl;
    std::cout << "cam_para.cy = " << cam_para.cy << std::endl;
}

void print_pixel(Mat &src)
{
    for (int i = 0; i < src.rows; i+= 6) {
		ushort* val_dep_ptr = src.ptr<ushort>(i);
        for (int j = 0; j < src.cols; j += 6) {
			ushort val_dep = val_dep_ptr[j];
			if(val_dep > 0) {
				printf("%d ", val_dep);
			}
		}
    }
	printf("\n");
}
#endif
